package cn.nexd.app.collector.collect;

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.text.format.Time;

/**
 * Created by wangxu on 16/9/9.
 */
public class SensorDetector {

    private Sensor sensor_type_accelerometer;
    private Sensor sensor_type_linear_acceleration;
    private Sensor sensor_type_gyroscope;
    private Sensor sensor_type_gravity;
    private Sensor sensor_type_magnetic_field;
    private Sensor sensor_type_rotation_vector;
    private Sensor sensor_type_orientation;
    private Sensor sensor_type_pressure;
    private Sensor sensor_type_ambient_temperature;
    private Sensor sensor_type_light;
    private Sensor sensor_type_proximity;
    private Sensor sensor_type_relative_humidity;
    private Sensor sensor_type_game_rotation_vector;
    private Sensor sensor_type_geomagnetic_rotation_vector;
    private Sensor sensor_type_step_counter;
    private Sensor sensor_type_step_detector;
    private Sensor sensor_type_magnetic_field_uncalibrated;
    private Sensor sensor_type_gyroscope_uncalibrated;


    private DefaultSensorEventListenerForCollector defaultSensorEventListenerForCollector;
    private SensorManager sensorManager;
    private float[] gyroMatrix;
    private float[] dataVector;
    private static final double NS2S = 1.0 / 1000000000.0;
    private final float[] deltaRotationVector = new float[4];
    private StepDetector stepDetector;

    private long counter = 0;
    private float lastStepCount = -1;
    private long timestamp = 0;
    private int counterCache = 0;
    private SensorCollectListener sensorCollectListener;

    public SensorDetector(Context context) {
        stepDetector = new StepDetector();
        sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE);
        defaultSensorEventListenerForCollector = new DefaultSensorEventListenerForCollector(context);
        dataVector = new float[69];
        gyroMatrix = new float[]{1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};
        initSensor();
        registerListener();
    }

    public void setSensorCollectListener(SensorCollectListener sensorCollectListener) {
        this.sensorCollectListener = sensorCollectListener;
    }


    private void initSensor() {
        sensor_type_accelerometer = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        sensor_type_linear_acceleration = sensorManager.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION);
        sensor_type_gyroscope = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
        sensor_type_gravity = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
        sensor_type_magnetic_field = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
        sensor_type_rotation_vector = sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
        sensor_type_orientation = sensorManager.getDefaultSensor(Sensor.TYPE_ORIENTATION);
        sensor_type_pressure = sensorManager.getDefaultSensor(Sensor.TYPE_PRESSURE);
        sensor_type_ambient_temperature = sensorManager.getDefaultSensor(Sensor.TYPE_AMBIENT_TEMPERATURE);
        sensor_type_light = sensorManager.getDefaultSensor(Sensor.TYPE_LIGHT);
        sensor_type_proximity = sensorManager.getDefaultSensor(Sensor.TYPE_PROXIMITY);
        sensor_type_relative_humidity = sensorManager.getDefaultSensor(Sensor.TYPE_RELATIVE_HUMIDITY);
        sensor_type_game_rotation_vector = sensorManager.getDefaultSensor(Sensor.TYPE_GAME_ROTATION_VECTOR);
        sensor_type_geomagnetic_rotation_vector = sensorManager.getDefaultSensor(Sensor.TYPE_GEOMAGNETIC_ROTATION_VECTOR);
        sensor_type_step_counter = sensorManager.getDefaultSensor(Sensor.TYPE_STEP_COUNTER);
        sensor_type_step_detector = sensorManager.getDefaultSensor(Sensor.TYPE_STEP_DETECTOR);
        sensor_type_magnetic_field_uncalibrated = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD_UNCALIBRATED);
        sensor_type_gyroscope_uncalibrated = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE_UNCALIBRATED);
    }

    public void unRegisterListener() {
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_accelerometer);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_linear_acceleration);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_gyroscope);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_gravity);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_magnetic_field);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_rotation_vector);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_orientation);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_pressure);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_ambient_temperature);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_light);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_proximity);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_relative_humidity);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_game_rotation_vector);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_geomagnetic_rotation_vector);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_step_counter);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_step_detector);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_magnetic_field_uncalibrated);
        sensorManager.unregisterListener(defaultSensorEventListenerForCollector, sensor_type_gyroscope_uncalibrated);
    }

    public void registerListener() {
        int sensorDelayNormal = 20000;
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_accelerometer, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_linear_acceleration, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_gyroscope, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_gravity, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_magnetic_field, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_rotation_vector, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_orientation, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_pressure, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_ambient_temperature, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_light, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_proximity, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_relative_humidity, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_game_rotation_vector, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_geomagnetic_rotation_vector, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_step_counter, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_step_detector, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_magnetic_field_uncalibrated, sensorDelayNormal);
        sensorManager.registerListener(defaultSensorEventListenerForCollector, sensor_type_gyroscope_uncalibrated, sensorDelayNormal);
    }


    public class DefaultSensorEventListenerForCollector implements SensorEventListener {

        public DefaultSensorEventListenerForCollector(Context context) {
        }

        private volatile int step;

        @Override
        public void onSensorChanged(SensorEvent event) {
            switch (event.sensor.getType()) {
                case Sensor.TYPE_ACCELEROMETER:
                    //数组 dataVector 的第 11 12 13 个元素
                    System.arraycopy(event.values, 0, dataVector, 10, 3);
                    SensorDetector.this.timestamp = event.timestamp;
                    Time t = new Time();
                    t.setToNow();
                    dataVector[43] = stepDetector.detect(event.values[2] / SensorManager.STANDARD_GRAVITY);
                    if (dataVector[43] == 1 || dataVector[56] != lastStepCount) {
                        lastStepCount = dataVector[56];
                        step = stepDetector.getStepCount();
                    }

                    if (counterCache >= 20) {
                        if (sensorCollectListener != null) {
                            sensorCollectListener.onSensorCycle(step, dataVector, counter, timestamp);
                            counter = counter + 1;
                        }
                    }
                    counterCache = counterCache + 1;
                    break;
                case Sensor.TYPE_LINEAR_ACCELERATION:
                    System.arraycopy(event.values, 0, dataVector, 13, 3);
                    break;
                case Sensor.TYPE_GYROSCOPE:
                    gyroHandler(event);
                    break;
                case Sensor.TYPE_GRAVITY:
                    System.arraycopy(event.values, 0, dataVector, 16, 3);
                    break;
                case Sensor.TYPE_MAGNETIC_FIELD:
                    System.arraycopy(event.values, 0, dataVector, 25, 3);
                    break;
                case Sensor.TYPE_ROTATION_VECTOR:
                    System.arraycopy(event.values, 0, dataVector, 28, 3);
                    break;
                case Sensor.TYPE_ORIENTATION:
                    System.arraycopy(event.values, 0, dataVector, 31, 3);
                    break;
                case Sensor.TYPE_PRESSURE:
                    System.arraycopy(event.values, 0, dataVector, 35, 1);
                    break;
                case Sensor.TYPE_AMBIENT_TEMPERATURE:
                    System.arraycopy(event.values, 0, dataVector, 36, 1);
                    break;
                case Sensor.TYPE_LIGHT:
                    System.arraycopy(event.values, 0, dataVector, 37, 1);
                    break;
                case Sensor.TYPE_PROXIMITY:
                    System.arraycopy(event.values, 0, dataVector, 38, 1);
                    break;
                case Sensor.TYPE_RELATIVE_HUMIDITY:
                    System.arraycopy(event.values, 0, dataVector, 39, 1);
                    break;
                case Sensor.TYPE_GAME_ROTATION_VECTOR:
                    System.arraycopy(event.values, 0, dataVector, 50, 3);
                    break;
                case Sensor.TYPE_GEOMAGNETIC_ROTATION_VECTOR:
                    System.arraycopy(event.values, 0, dataVector, 53, 3);
                    break;
                case Sensor.TYPE_STEP_COUNTER:
                    System.arraycopy(event.values, 0, dataVector, 56, 1);
                    break;
                case Sensor.TYPE_STEP_DETECTOR:
                    break;
                case Sensor.TYPE_MAGNETIC_FIELD_UNCALIBRATED:
                    System.arraycopy(event.values, 0, dataVector, 57, 6);
                    break;
                case Sensor.TYPE_GYROSCOPE_UNCALIBRATED:
                    System.arraycopy(event.values, 0, dataVector, 63, 6);
                    break;
                case Sensor.TYPE_SIGNIFICANT_MOTION:
                    break;

            }
        }

        @Override
        public void onAccuracyChanged(Sensor sensor, int accuracy) {

        }

        protected void gyroHandler(SensorEvent event) {

            // This timestep's delta rotation to be multiplied by the current
            // rotation
            // after computing it from the gyro sample data.
            if (timestamp != 0) {
                final double dT = (double) (event.timestamp - timestamp) * NS2S;
                // Axis of the rotation sample, not normalized yet.
                float axisX = event.values[0];
                float axisY = event.values[1];
                float axisZ = event.values[2];

                System.arraycopy(event.values, 0, dataVector, 19, 3);
                for (int i = 0; i < 3; i++) {
                    dataVector[44 + i] += (double) event.values[i] * dT;
                }
                // Calculate the angular speed of the sample
                float omegaMagnitude = (float) Math.sqrt(axisX * axisX + axisY
                        * axisY + axisZ * axisZ);

                // Normalize the rotation vector if it's big enough to get the
                // axis
                // (that is, EPSILON should represent your maximum allowable
                // margin of error)
                if (omegaMagnitude > Float.MIN_NORMAL) {
                    axisX /= omegaMagnitude;
                    axisY /= omegaMagnitude;
                    axisZ /= omegaMagnitude;
                }

                // Integrate around this axis with the angular speed by the
                // timestep
                // in order to get a delta rotation from this sample over the
                // timestep
                // We will convert this axis-angle representation of the delta
                // rotation
                // into a quaternion before turning it into the rotation matrix.
                float thetaOverTwo = (float) (omegaMagnitude * dT / 2.0f);
                float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
                float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);
                deltaRotationVector[0] = sinThetaOverTwo * axisX;
                deltaRotationVector[1] = sinThetaOverTwo * axisY;
                deltaRotationVector[2] = sinThetaOverTwo * axisZ;
                deltaRotationVector[3] = cosThetaOverTwo;
            }
            timestamp = event.timestamp;
            float[] deltaRotationMatrix = new float[9];
            float[] orientation = new float[3];
            SensorManager.getRotationMatrixFromVector(deltaRotationMatrix,
                    deltaRotationVector);
            gyroMatrix = matrixMultiplication(gyroMatrix, deltaRotationMatrix);
            //        System.arraycopy(gyroMatrix, 0, dataVector, 1, 9);
            // User code should concatenate the delta rotation we computed with
            // the current rotation
            // in order to get the updated rotation.
            // rotationCurrent = rotationCurrent * deltaRotationMatrix;
            SensorManager.getOrientation(gyroMatrix, orientation);
            System.arraycopy(orientation, 0, dataVector, 22, 3);
            // gyroYawTextView.setText(String.valueOf(orientation[0]));
            // gyroPitchTextView.setText(String.valueOf(orientation[1]));
            // gyroRollTextView.setText(String.valueOf(orientation[2]));
        }
    }


    private float[] matrixMultiplication(float[] A, float[] B) {
        float[] result = new float[9];

        result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
        result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
        result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];

        result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
        result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
        result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];

        result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
        result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
        result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];

        return result;
    }

}
